#!/usr/bin/env python
# coding:utf-8

import rospy
import serial
from geometry_msgs.msg import Twist
from std_msgs.msg import String

def cmd_vel(msg):

    global handle_sign
    global linear
    global angular

    if not isinstance(msg, Twist): return 0

    linear = msg.linear.x
    angular = msg.angular.z
    vel_tuple = (angular, linear, handle_sign)
    # data = (str(vel_tuple) + "/n").encode("utf-8")
    # ser.write(data)

    rospy.loginfo("vel_tuple = ({}, {}, {})".format(angular, linear, handle_sign))


def plant_Callback(msg):
    global handle_sign
    if not isinstance(msg, String): return 0

    handle_sign = msg.data


    rospy.loginfo("vel_tuple = ({}, {}, {})".format(angular, linear, handle_sign))





if __name__ == '__main__':

    # 创建节点
    rospy.init_node('my_driver_node')

    # 串口通信
    port = rospy.get_param("~driver_port", "/dev/driver" )
    ser = serial.Serial(port=port, baudrate=115200)

    twist_pub = rospy.Publisher('/zxcar/get_vel', Twist, queue_size=10)

    handle_sign = "99"

    # 控制指令
    rospy.Subscriber("plant", String, plant_Callback)

    rospy.Subscriber("cmd_vel", Twist, cmd_vel)


    # 和下位机进行通讯
    while not rospy.is_shutdown():
    
        data = ser.readline()
        try:
            # 阻塞式的函数
            # 读取并解码数据，变成元组
            data = ser.readline()
            data = data.decode("utf-8")
            data = str(data)[:-1] # uart串口字符分割，将无关字符去除 “\n” 和 b'' 去除
            print(data)


            data = eval(data) # 转化回元组形式
            # print(type(data))


        except SyntaxError or TypeError:
            data = None


        if data != None:
            # 提取速度
            velocity_data = data

            # 发布角速度线速度数据
            try:
                twist = Twist()
                # print("velocity_data:", velocity_data)
                angular_vel = velocity_data[0] # 角速度
                linear_vel = velocity_data[1] # 线速度
                twist.linear.x = linear_vel
                twist.linear.y = 0
                twist.linear.z = 0
                twist.angular.x = 0
                twist.angular.y = 0
                twist.angular.z = -angular_vel
                twist_pub.publish(twist)

            except TypeError:
                pass

        rospy.spin()
